#include "compass.h"

//Compass compass;

//unsigned short   compass_raw[3];   // raw data from compass
//unsigned short   compass_zero[3];  // calibration data
//float            compass_scale[3]; // calibration data

void compass_init(void)
{
  // Default compass_zero initialization
  compass.compass_zero[0] = IMU_ADC_COMPASSX_ZERO;
  compass.compass_zero[1] = IMU_ADC_COMPASSY_ZERO;
  compass.compass_zero[2] = IMU_ADC_COMPASSZ_ZERO;
  
  // Default compass_scale initialization
  compass.compass_scale[0] = IMU_ADC_COMPASSX_SCALE;
  compass.compass_scale[1] = IMU_ADC_COMPASSY_SCALE;
  compass.compass_scale[2] = IMU_ADC_COMPASSZ_SCALE;
}

void compass_update_x(unsigned short compass_raw_x)
{
  mag[0] = IMU_ADC_COMPASSX_SIGN ((float)compass_raw_x - compass.compass_zero[0]) * compass.compass_scale[0];
}

void compass_update_y(unsigned short compass_raw_y)
{
  mag[1] = IMU_ADC_COMPASSY_SIGN ((float)compass_raw_y - compass.compass_zero[1]) * compass.compass_scale[1];
}

void compass_update_z(unsigned short compass_raw_z)
{
  mag[2] = IMU_ADC_COMPASSZ_SIGN ((float)compass_raw_z - compass.compass_zero[2]) * compass.compass_scale[2];
}

void compass_calibrate(Compass* compass_calibration)
{
  compass.compass_zero[0] += (compass_calibration->compass_zero[0] / compass.compass_scale[0]);
  compass.compass_zero[1] += (compass_calibration->compass_zero[1] / compass.compass_scale[1]);
  compass.compass_zero[2] +=  (compass_calibration->compass_zero[2] / compass.compass_scale[2]);
  
  compass.compass_scale[0] *= compass_calibration->compass_scale[0];
  compass.compass_scale[1] *= compass_calibration->compass_scale[0];
  compass.compass_scale[2] *= compass_calibration->compass_scale[0];
}


